#include "eye_hand.h"
#include <thread>
#include "TransPoint.hpp"
#define SRCWND_NAME "src"      //原图像窗口名称
#define RESWND_NAME "eye_hand" //结果图像窗口名称
Mat srcImg, resImg, reslutImage;

void drawCross(Mat& img,Point point)
{
	//绘制横线
	cv::line(img,Point(point.x-6/2,point.y),\
             Point(point.x+6/2,point.y),Scalar(0,255,0),1,8,0);
	//绘制竖线
	cv::line(img,Point(point.x,point.y-6/2),\
             Point(point.x,point.y+6/2),Scalar(0,255,0),1,8,0);
	return;
}

void  EyeHand::callBack(int th, void *cam)
{
    EyeHand *eh = (EyeHand*)cam;
    eh->m_imgPoints.clear();
    try
    {
        cvtColor(srcImg, resImg, COLOR_BGR2GRAY);
        threshold(resImg, resImg, th, 255, THRESH_BINARY_INV | THRESH_OTSU);
        // 开运算去噪声
        Mat kernel = getStructuringElement(MORPH_ELLIPSE, Size(3, 3), Point(-1, -1));
        morphologyEx(resImg, resImg, MORPH_OPEN, kernel, Point(-1, -1));
        vector<vector<Point>> contours;
        vector<Vec4i> hireachy;
        reslutImage = srcImg.clone();
        //筛选标志点
        findContours(resImg, contours, hireachy, RETR_TREE, CHAIN_APPROX_SIMPLE, Point());
        int count = 1;//点的序号
        for (size_t t = 0; t < contours.size(); t++)
        {
            double area = contourArea(contours[t]);
            //cout <<"area:"<< area <<endl;
            //面积筛选
            if (area > 6000 || area < 800)
            {
                continue;
            }
            // 通过宽高比进行过滤
            Rect rect = boundingRect(contours[t]);
            float ratio = float(rect.width) / float(rect.height);
            if (ratio > 1.04 || ratio < 0.94)
            {
                continue;
            }
            if (eh->m_imgPoints.size() < 9)
            {
                // -1 填充，1画圆
                drawContours(reslutImage, contours, t, Scalar(0, 0, 255), 2, 8, Mat(), 0, Point());
                // printf("circle area : %f \n", area);
                //计算中心并且保存
                Moments mm = moments(contours[t]);
                float x = mm.m10 / mm.m00;
                float y = mm.m01 / mm.m00;
                eh->m_imgPoints.push_back(Point2f(x, y));
                cout <<"image point " << to_string(count) << ": x:"<<x<<" y:" <<y<<endl;
                putText(reslutImage, to_string(count), Point(x - rect.width / 3 * 2, y - rect.height / 2), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255, 0, 0), 2);
                // circle(reslutImage, Point(x, y), 1, Scalar(0, 0, 255), 1, 8, 0);
                drawCross(reslutImage, Point(x, y));
                count++;
            }
        }
        eh->PrintLog(string("总共提取到Mark点: ")+to_string(eh->m_imgPoints.size()) + "\n如果标志点提取正确，请在‘eye_hand’窗口按任意键继续...");

        cv::imshow(SRCWND_NAME, reslutImage);
        cv::imshow(RESWND_NAME, resImg);
    }
    
    catch (...)
    {
    }
}

int main(int argc, char *argv[])
{
    EyeHand eye_hand;
    VideoCapture camera;
    bool isok = false;
    if (argc < 2)
    {
        isok = camera.open(4);
    }
    else
    {
        cout<< argv[1]<<endl;
        isok = camera.open(atoi(argv[1]));
    }
    if (!isok)
    {
        eye_hand.PrintLog("相机打开失败，请重新启动程序,同时输入相机ID!!!");
        return 0;
    }
    //观测窗口和检测结果窗口
    namedWindow(SRCWND_NAME, WINDOW_NORMAL);
    namedWindow(RESWND_NAME, WINDOW_NORMAL);
    camera >> srcImg;
    resizeWindow(SRCWND_NAME, srcImg.cols / 2, srcImg.rows / 2);
    resizeWindow(RESWND_NAME, srcImg.cols / 2, srcImg.rows / 2);

    eye_hand.PrintLog("请调整标定板位置,确认标定板是在视野范围内...\n输入 y 开始标定\n输入 q 退出标定!");
    string strCmd;
    //检查标定板是否在视野中
    while (true)
    {
        bool quit = false;
        std::thread t1(
            [&]()
            {
                while (!quit)
                {
                    if (camera.read(srcImg))
                    {
                        try
                        {
                            // imwrite("test.jpg", m_frame);
                            imshow(SRCWND_NAME, srcImg);
                            imshow(RESWND_NAME, srcImg);
                            waitKey(30);
                        }
                        catch (...)
                        {
                            cout << "************  error **************" << endl;
                        }
                    }
                }
            });
        std::cin >> strCmd;
        getchar();
        if (string("y") == strCmd || string("Y") == strCmd)
        {
            quit = true;
            t1.join();
            break;
        }
        else if (string("q") == strCmd || string("Q") == strCmd)
        {
            quit = true;
            t1.join();
            eye_hand.PrintLog("      退出标定！！！    ");
            return 0;
        }
        else
        {
            quit = true;
            t1.join();
            continue;
        }
    }
    //开始处理图像：得到Mark点中心与轮廓
    int value = 50;
    createTrackbar("Mark点阈值:", RESWND_NAME, &value, 255, EyeHand::callBack, &eye_hand);
    waitKey();
    
    destroyWindow(RESWND_NAME);
    cv::imshow(SRCWND_NAME, reslutImage);
    thread t(
        [&]()
        {
            //标定
            Mat homat2d = eye_hand.Vector2Matrix();
            Point2f rms = eye_hand.RmsError(homat2d);
            char buff[1024];
            sprintf(buff, "重投影均方根误差值: x:%0.3f,y:%0.3f", rms.x, rms.y);
            eye_hand.PrintLog(buff);
        });
    t.join();
    eye_hand.PrintLog("完成标定,请在 “eye_hand” 按任意键继续...\n标定结果文件保存在当前路径...\n matrix.dat");
    waitKey();
    destroyAllWindows();
    return 0;
}



int main1()
{
    Homat2D mat;
    if(!loadMatrix(&mat))
    {
        cout<<"标定矩阵不存在"<<endl;
        return -1;
    }
    cout<<"Matrix:"<<endl;
    cout << mat.A << endl;
    cout << mat.B << endl;
    cout << mat.C << endl;
    cout << mat.D << endl;
    cout << mat.E << endl;
    cout << mat.F << endl;
    
    // VideoCapture cam(4);
    // Mat frame;
    // namedWindow("imgShow",WINDOW_NORMAL);
    // cam>>frame;
    // cvtColor(frame,frame,COLOR_BGR2GRAY);
    // threshold(frame,frame,50,255,THRESH_BINARY_INV|THRESH_OTSU);
    // imshow("imgShow",frame);
    // waitKey();
    // vector<vector<Point>> contours;
    // vector<Vec4i> hireachy;
    // //筛选标志点
    // findContours(frame, contours, hireachy, RETR_TREE, CHAIN_APPROX_SIMPLE, Point());
    // int count = 1; //点的序号
    // for (size_t t = 0; t < contours.size(); t++)
    // {
    //     double area = contourArea(contours[t]);
    //     //cout <<"area:"<< area <<endl;
    //     //面积筛选
    //     if (area < 45000 && area > 35000)
    //     {
    //         cout<<"--------point------"<<endl;
    //         Moments mm = moments(contours[t]);
    //         float x = mm.m10 / mm.m00;
    //         float y = mm.m01 / mm.m00;
    //         cout << x <<":" << y <<endl;
    //         Point2F p = affineTrans2DPoint(mat,Point2F{x,y});
    //         cout<< p.x <<":" <<p.y<<endl;
    //     }
    // }
    Point2F p = affineTrans2DPoint(mat,Point2F{690.003,563.715});
    cout<< p.x <<" : " <<p.y<<endl;
    return 0;
}
